
#include "Eigen/Core"
#include "Eigen/Geometry"

#include <iostream>
#include <string>

template <typename FloatType>
class Rigid2
{
public:
    using Affine     = Eigen::Transform<FloatType, 2, Eigen::Affine>;
    using Vector     = Eigen::Matrix<FloatType, 2, 1>;
    using Rotation2D = Eigen::Rotation2D<FloatType>;

    Rigid2()
        : translation_(Vector::Identity()), rotation_(Rotation2D::Identity()) {}
    Rigid2(const Vector &translation, const Rotation2D &rotation)
        : translation_(translation), rotation_(rotation) {}
    Rigid2(const Vector &translation, const double rotation)
        : translation_(translation), rotation_(rotation) {}

    static Rigid2 Rotation(const double rotation)
    {
        return Rigid2(Vector::Zero(), rotation);
    }

    static Rigid2 Rotation(const Rotation2D &rotation)
    {
        return Rigid2(Vector::Zero(), rotation);
    }

    static Rigid2 Translation(const Vector &vector)
    {
        return Rigid2(vector, Rotation2D::Identity());
    }

    static Rigid2<FloatType> Identity()
    {
        return Rigid2<FloatType>(Vector::Zero(), Rotation2D::Identity());
    }

    template <typename OtherType>
    Rigid2<OtherType> cast() const
    {
        return Rigid2<OtherType>(translation_.template cast<OtherType>(),
                                 rotation_.template cast<OtherType>());
    }

    const Vector &translation() const { return translation_; }

    Rotation2D rotation() const { return rotation_; }

    //   double normalized_angle() const {
    //     return common::NormalizeAngleDifference(rotation().angle());
    //   }

    Rigid2 inverse() const
    {
        const Rotation2D rotation = rotation_.inverse();
        const Vector translation  = -(rotation * translation_);
        return Rigid2(translation, rotation);
    }

    string DebugString() const
    {
        string out;
        out.append("{ t: [");
        out.append(std::to_string(translation().x()));
        out.append(", ");
        out.append(std::to_string(translation().y()));
        out.append("], r: [");
        out.append(std::to_string(rotation().angle()));
        out.append("] }");
        return out;
    }

private:
    Vector translation_;
    Rotation2D rotation_;
};

template <typename FloatType>
Rigid2<FloatType> operator*(const Rigid2<FloatType> &lhs,
                            const Rigid2<FloatType> &rhs)
{
    return Rigid2<FloatType>(
        lhs.rotation() * rhs.translation() + lhs.translation(),
        lhs.rotation() * rhs.rotation());
}

template <typename FloatType>
typename Rigid2<FloatType>::Vector operator*(
    const Rigid2<FloatType> &rigid,
    const typename Rigid2<FloatType>::Vector &point)
{
    return rigid.rotation() * point + rigid.translation();
}

// This is needed for gmock.
template <typename T>
std::ostream &operator<<(std::ostream &os,
                         const Rigid2<T> &rigid)
{
    os << rigid.DebugString();
    return os;
}

using Rigid2d = Rigid2<double>;
using Rigid2f = Rigid2<float>;